#include "rads_rtk.hpp"

// 全局原子变量，用于线程间的通信
std::atomic<bool> should_stop(false);
std::condition_variable cv;
std::mutex mtx;
// 差分数据来源模式
enum DiffDataSource
{
    QIANXUN_NETWORK, // 千寻网络差分
    SERIAL_PORT      // 串口差分数据
};
DiffDataSource diff_data_source;
// 主要是获取网络来源的差分数据
void threadrtk_qianxun(std_msgs::String &nema_msg, QxDiffData &diff_data, std_msgs::Int16 &rtcm)
{
    if (diff_data_source != QIANXUN_NETWORK)
        return;

    TCPSocket socket;
    socket.initialize();
    std::string genGPGGA = "";
    char check_buffer[2048];
    size_t check_length;
    bool connecting = false;
    bool qx_auth = false;

    while (!should_stop.load())
    {
        if (!connecting)
        {
            // 千寻服务初始化
            if (socket.connectToServer(QX_SERVER, QX_PORT))
            {
                connecting = true;
                std::string auth = authCode();
                if (socket.sendData(auth.c_str(), auth.size()))
                {
                    if (socket.receiveData(check_buffer, sizeof(check_buffer), check_length))
                    {
                        std::string checkCode = check_buffer;
                        if (0 == checkCode.find(ICY_200_OK))
                            qx_auth = true;
                    }
                }
            }
        }

        if (qx_auth)
        {
            // 差分数据收发服务
            std::vector<std::string> result = splitString(nema_msg.data);
            if (GPYBM_LENGTH == result.size())
            {
                NEMA_GPYBM gpybm = nema2gpybm(result);
                if (std::isnan(safe_stod(gpybm.lat)) || std::isnan(safe_stod(gpybm.lng)))
                {
#ifdef DEV_LOCAL
                    ROS_ERROR("Judging the vehicle may be inside,NEMA_GPYBM is NaN");
#endif
                }
                else
                {
                    genGPGGA = generateGPGGA(safe_stod(gpybm.lat), safe_stod(gpybm.lng), gpybm.fix_master);
                    socket.sendData(genGPGGA.c_str(), genGPGGA.size());
                    socket.receiveData(diff_data.data, DIFF_DATA_LEN, diff_data.len);
                    rtcm.data = diff_data.len;
                    diff_data.is_ready = 1;
                }
            }
        }
        std::unique_lock<std::mutex> lck(mtx);
        cv.wait_for(lck, std::chrono::milliseconds(1000), []
                    { return should_stop.load(); });
    }
}
// ========== CRC24Q 校验 ==========
static uint32_t crc24q(const uint8_t *data, int len)
{
    const uint32_t poly = 0x1864CFB;
    uint32_t crc = 0;
    for (int i = 0; i < len; ++i)
    {
        crc ^= (uint32_t)data[i] << 16;
        for (int j = 0; j < 8; ++j)
        {
            crc <<= 1;
            if (crc & 0x1000000)
                crc ^= poly;
        }
    }
    return crc & 0xFFFFFF;
}

// ========== 处理帧 ==========

void processRtcmFrame(const std::vector<uint8_t> &frame, QxDiffData &diff_data, SerialPortRtk &serial)
{
    ROS_INFO("[OK] RTCM frame received, length, length = %d", frame.size());
    serial.writeLine(reinterpret_cast<const char *>(frame.data()), frame.size());
}

// ========== 串口初始化函数 ==========
bool configureSerialPort(int fd, int baudrate = B115200)
{
    struct termios tty;
    memset(&tty, 0, sizeof tty);

    if (tcgetattr(fd, &tty) != 0)
    {
        perror("tcgetattr");
        return false;
    }

    // 设置输入/输出波特率
    cfsetospeed(&tty, baudrate);
    cfsetispeed(&tty, baudrate);

    // 控制模式：8N1
    tty.c_cflag = (tty.c_cflag & ~CSIZE) | CS8; // 8位数据
    tty.c_cflag &= ~PARENB;                     // 无奇偶校验
    tty.c_cflag &= ~CSTOPB;                     // 1位停止位
    tty.c_cflag &= ~CRTSCTS;                    // 禁用硬件流控
    tty.c_cflag |= CREAD | CLOCAL;              // 打开接收器，忽略控制线

    // 本地模式：原始输入模式（非规范输入）
    tty.c_lflag &= ~(ICANON | ECHO | ECHOE | ISIG);

    // 输入模式：禁用流控和特殊字符解析
    tty.c_iflag &= ~(IXON | IXOFF | IXANY);
    tty.c_iflag &= ~(IGNBRK | BRKINT | PARMRK | ISTRIP | INLCR | IGNCR | ICRNL);

    // 输出模式：原始输出
    tty.c_oflag &= ~OPOST;

    // 设置读取超时（非阻塞方式）
    tty.c_cc[VMIN] = 1;  // 至少读取1个字节
    tty.c_cc[VTIME] = 1; // 读取超时时间 0.1s

    if (tcsetattr(fd, TCSANOW, &tty) != 0)
    {
        perror("tcsetattr");
        return false;
    }

    return true;
}

// 从串口读取差分数据
void threadrtk_serial_diff(QxDiffData &diff_data, std_msgs::Int16 &rtcm, SerialPortRtk &serial)
{
    if (diff_data_source != SERIAL_PORT)
        return;

    int fd = -1;
    const int retry_delay_ms = 1000; // 1秒重试间隔

    // 无限重试直到成功或ros退出
    while (ros::ok() && fd < 0)
    {
        fd = open(RTK_DEVICES.c_str(), O_RDONLY | O_NOCTTY);
        if (fd < 0)
        {
            perror("open");
            std::cerr << "Retrying to open serial port in " << retry_delay_ms << "ms..." << std::endl;
            usleep(retry_delay_ms * 1000);
        }
    }

    if (!ros::ok()) {
        return; // 如果ros退出则直接返回
    }

    if (!configureSerialPort(fd))
    {
        close(fd);
        return;
    }

    enum State
    {
        WAIT_HEADER,
        READ_HEADER,
        READ_PAYLOAD_CRC
    };

    State state = WAIT_HEADER;
    std::vector<uint8_t> frameBuffer;
    int payloadLen = 0;

    while (ros::ok())
    {
        uint8_t byte;
        int n = read(fd, &byte, 1);
        if (n <= 0)
            continue;

        switch (state)
        {
        case WAIT_HEADER:
            if (byte == 0xD3)
            {
                frameBuffer.clear();
                frameBuffer.push_back(byte);
                state = READ_HEADER;
            }
            break;

        case READ_HEADER:
            frameBuffer.push_back(byte);
            if (frameBuffer.size() == 3)
            {
                payloadLen = ((frameBuffer[1] & 0x03) << 8) | frameBuffer[2];
                state = READ_PAYLOAD_CRC;
            }
            break;

        case READ_PAYLOAD_CRC:
            frameBuffer.push_back(byte);
            if (frameBuffer.size() == 3 + payloadLen + 3)
            {
                int frameLen = frameBuffer.size();
                uint32_t crc_calc = crc24q(frameBuffer.data(), 3 + payloadLen);
                uint32_t crc_recv =
                    (frameBuffer[frameLen - 3] << 16) |
                    (frameBuffer[frameLen - 2] << 8) |
                    (frameBuffer[frameLen - 1]);

                if (crc_calc == crc_recv)
                {
                    processRtcmFrame(frameBuffer, std::ref(diff_data), std::ref(serial));
                }
                else
                {
                    std::cerr << "[CRC ERROR] Bad frame. Expected 0x" << std::hex << crc_calc
                              << ", got 0x" << crc_recv << std::dec << "\n";
                }
                state = WAIT_HEADER;
            }
            break;
        }
    }
    close(fd);
}

// 读取串口NEMA_GPGGA数据并且输入差分数据
void threadrtk_core(ros::NodeHandle &nh, std_msgs::String &nema_msg, QxDiffData &diff_data, rads::rads_trackext &nav, SerialPortRtk &serial)
{
    bool serial_dev_status = false;
    std::string line;
    while (!should_stop.load())
    {
        try
        {
            if (!serial_dev_status)
            {
                if (serial.open())
                {
                    serial_dev_status = true;
                }
                else
                {
                    ROS_WARN("[rtk] is running,but serial open [%s] fail.", std::string(RTK_DEVICE).c_str());
                }
                std::this_thread::sleep_for(std::chrono::seconds(RTK_CHECK_SEC));
            }
            else
            {
                if (serial.readLine(line))
                {
                    if (serial.startsWith(line, PREFIX_GPYBM))
                    {
                        std::vector<std::string> result = splitString(line);
                        if (GPYBM_LENGTH == result.size())
                        {
                            NEMA_GPYBM gpybm = nema2gpybm(result);
                            nema_msg.data = line;
                            // 经纬度，解算情况，航向
                            nav.latitude = safe_stod(gpybm.lat);
                            nav.longitude = safe_stod(gpybm.lng);
                            nav.altitude = safe_stod(gpybm.alt);
                            nav.posi_flag = gpybm.fix_master;
                            nav.head_flag = gpybm.fix_slave;
                            nav.heading = safe_stod(gpybm.yaw);
                            nav.yaw = safe_stod(gpybm.yaw);
                            nav.groud_speed = gpybm.spd_groud;
                            nav.matching_rate = -1;
                            // 全局北向X,全局东向
                            nav.x = gpybm.gauss_nx;
                            nav.y = gpybm.gauss_ey;
                            nav.z = safe_stod(gpybm.alt);
                            // 地向速度(ext)
                            nav.ext = gpybm.spd_groud;
                            // 基线长度(flag)
                            nav.flag = gpybm.base_length;

                            if (diff_data.is_ready)
                            {
#if CURRENT_DEBUG_LEVEL > DEBUG_LEVEL_INFO
                                // ROS_INFO("Sending differential data size(%zu) .",diff_data.len);
#endif
                                serial.writeLine(diff_data.data, diff_data.len);
                                diff_data.is_ready = 0;
                            }
                        }
                    }
                    std::unique_lock<std::mutex> lck(mtx);
                    cv.wait_for(lck, std::chrono::milliseconds(1), []
                                { return should_stop.load(); });
                }
                else
                {
                    std::cerr << "Failed to read line from serial port" << std::endl;
                    break;
                }
            }
        }
        catch (const std::exception &e)
        {
            ROS_ERROR("Thread caught an exception: %s", e.what());
            break;
        }
        catch (...)
        {
            ROS_ERROR("Thread caught an unknown exception");
            break;
        }
    }
}

// 信号处理函数
void signalHandler(int signum)
{
    if (SIGINT == signum)
    {
        system("killall -s SIGTERM rads_guard");
        std::this_thread::sleep_for(std::chrono::milliseconds(20));
        ROS_INFO("[Rtk] Interrupt signal (%d) received. Shutting down...", signum);
        std::lock_guard<std::mutex> lck(mtx);
        should_stop.store(true);
        cv.notify_all();
        ros::shutdown();
    }
}

int main(int argc, char **argv)
{
    ROS_INFO("[Rtk] Starting...");
    ros::init(argc, argv, NODE_RTK);
    ros::NodeHandle nh;
    ros::param::get("RTK_DEVICE", RTK_DEVICE);
    ros::param::get("RTK_DEVICES", RTK_DEVICES);
    ros::param::get("RTK_F", RTK_F);
    ros::param::get("QX_USER", QX_USER);
    ros::param::get("QX_USERPASS", QX_USERPASS);
    if (RTK_F == 1)
    {
        diff_data_source = QIANXUN_NETWORK; // 千寻网络
    }
    else if (RTK_F == 2)
    {
        diff_data_source = SERIAL_PORT; // 串口差分数据
    }
    // 从参数服务器获取差分数据来源
    std::string diff_source;
    ros::param::param<std::string>("diff_data_source", diff_source, "qianxun");
    if (diff_data_source == SERIAL_PORT)
    {
        diff_data_source = SERIAL_PORT;
        ROS_INFO("Using serial port SERIAL_PORT for differential data");
    }
    else
    {
        diff_data_source = QIANXUN_NETWORK;
        ROS_INFO("Using Qianxun network service for differential data");
    }
    // 设置信号处理函数
    signal(SIGINT, signalHandler);
    std_msgs::String nema0183_gpybm;
    QxDiffData diff_data;
    rads::rads_trackext nav;
    std_msgs::Int16 rtcm;
    // 获取GNSS定位数据
    SerialPortRtk serial(RTK_DEVICE, B115200);
    std::thread t_rtk(threadrtk_core, std::ref(nh), std::ref(nema0183_gpybm), std::ref(diff_data), std::ref(nav), std::ref(serial));
    // 差分数据来源线程
    std::thread t_diff_source;
    if (diff_data_source == QIANXUN_NETWORK)
    {
        t_diff_source = std::thread(threadrtk_qianxun, std::ref(nema0183_gpybm), std::ref(diff_data), std::ref(rtcm));
    }
    else
    {
        t_diff_source = std::thread(threadrtk_serial_diff, std::ref(diff_data), std::ref(rtcm), std::ref(serial));
    }
    // NAV定位基本信息话题发布
    ros::Publisher p = nh.advertise<rads::rads_trackext>("/pradsrtk", QUEUE_SIZE);
    // 话题发布的rtcm字节数
    ros::Publisher p2 = nh.advertise<std_msgs::Int16>("/pradsrtcm", QUEUE_SIZE);
    ros::Rate rate(5);
    while (nh.ok() && !should_stop.load())
    {
        nav.header.stamp = ros::Time::now();
        nav.header.frame_id = "k827";
        p.publish(nav);
        p2.publish(rtcm);
        ros::spinOnce();
        rate.sleep();
    }

    if (t_rtk.joinable())
        t_rtk.join();
    if (t_diff_source.joinable())
        t_diff_source.join();

    return 0;
}